#include <fstream>
#include <MMM/Exceptions.h>
#include <MMM/RapidXML/RapidXMLReader.h>
#include <MMM/XMLTools.h>
#include <boost/algorithm/string.hpp>

#include "DataGloveConverter.h"

using namespace MMM;

FingerMapping::FingerMapping(unsigned int index, const std::string &jointName, const std::string &description, float multiplyBy) :
    index(index),
    jointName(jointName),
    description(description),
    multiplyBy(multiplyBy)
{
}

std::vector<std::string> FingerMapping::getJointNames(const FingerMappingList &mappings) {
    std::vector<std::string> jointNames;
    for (auto mapping : mappings) {
        jointNames.push_back(mapping->jointName);
    }
    return jointNames;
}

float FingerMapping::getJointAngle(const std::vector<std::string> &values, int row) {
    if (index >= values.size()) throw Exception::MMMException("Index " + std::to_string(index) + " needs to be smaller than column size " + std::to_string(values.size()));
    std::string jointAngleStr = values[index];
    std::replace(jointAngleStr.begin(), jointAngleStr.end(), ',', '.');
    return XML::convertTo<float>(jointAngleStr, "'" + values[index] + "' is no valid float at row " + std::to_string(row) + " and column " + std::to_string(index) + "!") * multiplyBy;
}

DataGloveConverter::DataGloveConverter(const std::string &dataGloveConfigPath) : separator(";"), hasHeader(false), useTimestepIndex(true) {
    loadConfig(dataGloveConfigPath);
}

/** @throws MMMException */
KinematicSensorPtr DataGloveConverter::convert(const std::string &dataGloveDataPath) {
    KinematicSensorPtr sensor(new KinematicSensor(FingerMapping::getJointNames(fingerMapping)));
    std::ifstream infile(dataGloveDataPath);
    if (infile.is_open()) {
        std::string line;
        if (hasHeader) std::getline(infile, line); // Ignore first line
        int count = 0; // used for Delta
        while (std::getline(infile, line))
        {
            std::vector<std::string> values;
            boost::split(values, line, boost::is_any_of(separator));

            if (timestepIndex >= values.size()) throw Exception::MMMException("Index " + std::to_string(timestepIndex) + " needs to be smaller than column size " + std::to_string(values.size()));
            std::string timestepStr = values[timestepIndex];
            // glove system time to milliseconds
            std::vector<std::string> splittedTime;
            std::string delimiter = ":";
            size_t pos = 0;
            while ((pos = timestepStr.find(delimiter)) != std::string::npos) {
                splittedTime.push_back(timestepStr.substr(0, pos));
                timestepStr.erase(0, pos + delimiter.length());
            }
            splittedTime.push_back(timestepStr.substr(0, 1));
            double timeInMs = MMM::XML::convertTo<int>(splittedTime.at(0)) * 60 * 60 * 1000 + MMM::XML::convertTo<int>(splittedTime.at(1)) * 60 * 1000 + MMM::XML::convertTo<int>(splittedTime.at(2)) * 1000 + MMM::XML::convertTo<int>(splittedTime.at(3))/30 * 1000 + MMM::XML::convertTo<int>(splittedTime.at(4)) * 1000/90 ;
            //std::replace(timestepStr.begin(), timestepStr.end(), ',', '.');
            float timestep = useTimestepIndex ? timeInMs//XML::convertTo<float>(timestepStr, "'" + values[timestepIndex] + " is no valid float for time at row " + std::to_string(count + hasHeader) + " and column " + std::to_string(timestepIndex) + "!")
                                              : ++count * timestepDelta;

            Eigen::VectorXf jointAngles(fingerMapping.size());
            int i = 0;
            for (auto mapping : fingerMapping) {
                jointAngles(i++) = mapping->getJointAngle(values, count + hasHeader);
            }

            KinematicSensorMeasurementPtr sensorMeasurement(new KinematicSensorMeasurement(timestep, jointAngles));
            sensor->addSensorMeasurement(sensorMeasurement);
        }
    }
    else throw MMM::Exception::MMMException(dataGloveDataPath + " could not be opened!");

    if (sensor->getTimesteps().size() == 0) throw MMM::Exception::MMMException("No measurements were added!");

    return sensor;
}

void DataGloveConverter::loadConfig(const std::string &dataGloveConfigPath) {
    RapidXMLReaderPtr reader = RapidXMLReader::FromFile(dataGloveConfigPath);
    RapidXMLReaderNodePtr rootNode = reader->getRoot("DataGloveConverterConfig");
    if (rootNode->has_node("DataGloveFileConfig")) {
        RapidXMLReaderNodePtr dataFileConfigNode = rootNode->first_node("DataGloveFileConfig");
        if (dataFileConfigNode->has_attribute("separator")) {
            separator = dataFileConfigNode->attribute_value("separator");
        }
        if (dataFileConfigNode->has_attribute("hasHeader")) {
            std::string hasHeaderStr = dataFileConfigNode->attribute_value("hasHeader");
            hasHeader = XML::convertToBool(hasHeaderStr, "The attribut value of 'hasHeader' needs to be a valid bool not '" + hasHeaderStr + "'");
        }
    }
    RapidXMLReaderNodePtr timestepNode = rootNode->first_node("Timestep");
    std::string timestepType = XML::toLowerCase(timestepNode->attribute_value("type").c_str());
    std::string timestepValue = timestepNode->attribute_value("value");
    if (timestepType == "byindex") {
        timestepIndex = XML::convertTo<unsigned int>(timestepValue, "If the type 'ByIndex' is used, the value attribute need to be a valid unsigned integer!");
        useTimestepIndex = true;
    } else if (timestepType == "bydelta") {
        timestepDelta = XML::convertTo<float>(timestepValue, "If the type 'ByDelta' is used, the value attribute need to be a valid float!");
        timestepIndex = 0; //test
        useTimestepIndex = false;
    }
    else throw Exception::XMLFormatException("'" + timestepType + "' is no valid timestep type attribute. Use 'ByIndex' or 'ByDelta'!");
    std::set<std::string> jointNames;
    for (RapidXMLReaderNodePtr mappingNode : rootNode->first_node("FingerMapping")->nodes("Mapping")) {
        float multiplyBy = 1.0f;
        std::string jointName = mappingNode->attribute_value("jointName");
        if (jointNames.find(jointName) != jointNames.end())
            throw Exception::XMLFormatException("Duplicate of mapping with jointName '" + jointName + "'");
        if (mappingNode->has_attribute("multiplyBy"))
            multiplyBy = XML::convertTo<float>(mappingNode->attribute_value("multiplyBy"), "The attribute value of 'multiplyBy' on mapping with joint name '" + jointName +"' need to be a valid float!");
        fingerMapping.push_back(FingerMappingPtr(new FingerMapping(XML::convertTo<unsigned int>(mappingNode->attribute_value("index"), "Index attribute for jointName " + jointName + " no valid unsigned integer!"),
                                                                   jointName,
                                                                   mappingNode->attribute_value_or_default("description", ""),
                                                                   multiplyBy)));
        jointNames.insert(jointName);
    }
}
